/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "attitude.h"
#include "pid.h"
#include "fc_sensors.h"
#include "estimator.h"
#include "imu.h"
#include "motor_mix.h"
#include "maths.h"
#include "rc_controls.h"
#include "motor_mix.h"
#include "fc_state.h"
#include "anoPTv8_report.h"
#include "position.h"
#include "maths.h"

#define DBG_TAG "alt"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define POSITION_P_SCALE 0.1f
#define POSITION_I_SCALE 0.01f
#define POSITION_D_SCALE 0.01f
#define MAX_VELOCITY_Z 200 // cm/s
#define MOTOR_WARMUP_THROTTLE 500
#define INTER_LIMIT 300

const pidf_t alt_pid_default[ALT_COUNT] = {
    {10.f, 0.0f, 0.0f},
    {20.f, 120.0f, 0.0f},
};
pidf_t alt_pid_param[ALT_COUNT];
PID_t alt_pid_obj[ALT_COUNT];

float alt_hold_throttle = 0;
static uint32_t manaulTz = 0;
static uint32_t interSaturationT = 0;
/**motor warmup , avoid current overload */
bool motor_warmup_ready = false;
// bool alt_hold_changing = false;
float alt_hold_pos = 0;

ZINO_CONFIG_REGISTER(struct altHoldPidConfig, altHoldPidConfig, 1, 0);

bool is_motor_warmup_ready(void)
{
    return motor_warmup_ready;
}
void position_z_pid_init(void)
{
    rt_memset(&alt_pid_param, 0, sizeof(alt_pid_param));
    if (altHoldPidConfig()->flag == ALT_SAVED_FLAG)
    {
        rt_memcpy(&alt_pid_param, &altHoldPidConfig()->pid, sizeof(alt_pid_param));
    }
    else
    {
        rt_memcpy(&alt_pid_param, &alt_pid_default, sizeof(alt_pid_param));
        rt_memcpy(&altHoldPidConfig()->pid, &alt_pid_param, sizeof(alt_pid_param));
    }
    pid_set_parameters(&alt_pid_obj[ALT_POS],
                       alt_pid_param[ALT_POS].P * POSITION_P_SCALE,
                       alt_pid_param[ALT_POS].I * POSITION_I_SCALE,
                       alt_pid_param[ALT_POS].D * POSITION_D_SCALE,
                       100, 200);
    pid_set_parameters(&alt_pid_obj[ALT_VEL],
                       alt_pid_param[ALT_VEL].P * POSITION_P_SCALE,
                       alt_pid_param[ALT_VEL].I * POSITION_I_SCALE,
                       alt_pid_param[ALT_VEL].D * POSITION_D_SCALE,
                       INTER_LIMIT, 400);

    alt_pid_obj[ALT_VEL].mode = PID_MODE_DERIVATIV_CALC;
    alt_pid_obj[ALT_POS].mode = PID_MODE_DERIVATIV_CALC;
}
void position_z_pid_save(void)
{
    altHoldPidConfig()->flag = ALT_SAVED_FLAG;
}
void position_z_pid_reload(void)
{
    if (altHoldPidConfig()->flag == ALT_SAVED_FLAG)
    {
        rt_memcpy(&alt_pid_param, &altHoldPidConfig()->pid, sizeof(alt_pid_param));

        alt_pid_obj[ALT_POS].kp = alt_pid_param[ALT_POS].P * POSITION_P_SCALE;
        alt_pid_obj[ALT_POS].ki = alt_pid_param[ALT_POS].I * POSITION_I_SCALE;
        alt_pid_obj[ALT_POS].kd = alt_pid_param[ALT_POS].D * POSITION_D_SCALE;

        alt_pid_obj[ALT_VEL].kp = alt_pid_param[ALT_VEL].P * POSITION_P_SCALE;
        alt_pid_obj[ALT_VEL].ki = alt_pid_param[ALT_VEL].I * POSITION_I_SCALE;
        alt_pid_obj[ALT_VEL].kd = alt_pid_param[ALT_VEL].D * POSITION_D_SCALE;

        LOG_D("alt pid reload");
        LOG_D("POS: %3d, I: %3d, D: %3d", alt_pid_param[ALT_POS].P, alt_pid_param[ALT_POS].I, alt_pid_param[ALT_POS].D);
        LOG_D("VEL: %3d, I: %3d, D: %3d", alt_pid_param[ALT_VEL].P, alt_pid_param[ALT_VEL].I, alt_pid_param[ALT_VEL].D);
    }
}
void position_z_pid_reset_default(void)
{
    rt_memcpy(&alt_pid_param, &alt_pid_default, sizeof(alt_pid_param));
    rt_memcpy(&altHoldPidConfig()->pid, &alt_pid_param, sizeof(alt_pid_param));
}
void position_z_control(float dt)
{
    float vel_z_setpoint = getRcDeflection(THROTTLE) * rc_control_max()->throttle;
    // float RC = 1 / (2 * M_PI_FLOAT * f_cut);
    // return dT / (RC + dT);

    if (ABS(vel_z_setpoint) > 1.f)
    {
        fc_setpoint.manual[Z] = true;
        fc_setpoint.vel.z = vel_z_setpoint;
        manaulTz = millis();
    }

    if (FC_STATE_CHECK(FC_ARMED))
    {
        if (!motor_warmup_ready)
        {
            if (currentMixer.throttle < MOTOR_WARMUP_THROTTLE)
            {
                currentMixer.throttle += 5;
            }
            else
            {
                motor_warmup_ready = true;
            }
        }
        else
        {
            currentMixer.throttle = alt_hold_throttle + MOTOR_WARMUP_THROTTLE;
        }
    }
    else
    {
        pid_reset_integral(&alt_pid_obj[ALT_POS]);
        pid_reset_integral(&alt_pid_obj[ALT_VEL]);
        motor_warmup_ready = false;
    }

    if (motor_warmup_ready)
    {
        if (fc_setpoint.manual[Z])
        {
            fc_setpoint.pos.z = zdrone_state.pos.z;
            if (millis() > (manaulTz + 100))
            {
                fc_setpoint.manual[Z] = false;
            }
        }
        else
        {
            fc_setpoint.vel.z = pid_calculate(&alt_pid_obj[ALT_POS], fc_setpoint.pos.z, zdrone_state.pos.z, 0, dt);
        }
        alt_hold_throttle = pid_calculate_lpf(&alt_pid_obj[ALT_VEL], fc_setpoint.vel.z, zdrone_state.vel.z, dt, 1.f);

        if (alt_pid_obj[ALT_VEL].integral > INTER_LIMIT * 0.85f)
        {
            if (interSaturationT == 0)
            {
                interSaturationT = millis();
            }
            else if (millis() > (interSaturationT + 1000))
            {
                /**NOTE throttle reach max, means power low*/
                FC_STATE_SET(FC_LOW_POWER);
            }
        }
        else
        {
            interSaturationT = 0;
        }
        // F1_wave[8] = fc_setpoint.pos.z;
        // F1_wave[9] = alt_pid_obj[ALT_POS].last_output;
        // F1_wave[10] = alt_pid_obj[ALT_VEL].last_output;
        // F1_wave[11] = alt_pid_obj[ALT_VEL].integral;
    }
    // F1_wave[8] = fc_setpoint.vel.z;
    // F1_wave[9] = alt_pid_obj[ALT_VEL].integral;
    // F1_wave[10] = alt_hold_throttle;
    // F1_wave[11] = zdrone_state.vel.z;
}
